#ifndef STATE_ESTIMATOR_H
#define STATE_ESTIMATOR_H

#include "Sensor.h"
#include "Modeling/Robot.h"
#include "ODERobot.h"

struct RobotStateEstimator
{
  RobotStateEstimator(Robot& _robot) :robot(_robot) {}
  virtual ~RobotStateEstimator() {}
  virtual void ReadSensors(const RobotSensors& sensors) {}
  virtual void UpdateModel() {}
  virtual void ReadCommand(const RobotCommand& command) {}
  virtual void Advance(Real dt) {}
  virtual void Reset();

  Robot& robot;
};

struct OmniscientStateEstimator : public RobotStateEstimator
{
  OmniscientStateEstimator(Robot& _robot,ODERobot& _oderobot)
    :RobotStateEstimator(_robot),oderobot(_oderobot) {}
  virtual ~OmniscientStateEstimator();
  virtual void UpdateModel() {
    oderobot.GetConfig(robot.q);
    oderobot.GetVelocities(robot.dq);
    robot.UpdateFrames();
  }

  ODERobot& oderobot;
};

#endif
